#include "bsp/bsp.h"
#include "bsp/cpu_driver.h"
#include "bsp/uart.h"
#include "utils/circle_buffer.h"
#include "stm32g4xx_ll_dma.h"

static hw_call_back rx_cb_;
static void* ctx_;
static c_buffer_t *tx_queue_;
static u8 uart_rx_cache[CONFIG_UART_RX_BUFFER_SIZE];
UART_HandleTypeDef huart3;
DMA_HandleTypeDef hdma_usart3_rx;
DMA_HandleTypeDef hdma_usart3_tx;
static u16 uart_tx_length = 0;
static u16 uart_rx_index = 0;
static void MX_USART3_UART_Init(void);
static void Error_Handler(void) {}

void uart_init(unsigned char hw_id, unsigned int rate, hw_call_back cb, void *ctx) {
	rx_cb_ = cb;
	ctx_ = ctx;
	uart_tx_length = 0;
	uart_rx_index = 0;
	MX_USART3_UART_Init();
}

#define DMA_IS_ENABLED(ins) LL_DMA_IsEnabledChannel(ins.DmaBaseAddress, ins.ChannelIndex)
#define DMA_FLAGS_GET(ins, f) __HAL_DMA_GET_FLAG(ins, f)
#define DMA_FLAGS_CLEAR(ins, f) __HAL_DMA_CLEAR_FLAG(ins, f)
#define DMA_SET_ENABLE(ins, en) if(en) {LL_DMA_EnableChannel(ins.DmaBaseAddress, ins.ChannelIndex);}else {LL_DMA_DisableChannel(ins.DmaBaseAddress, ins.ChannelIndex);}
#define DMA_SET_LENGTH(ins, len) LL_DMA_SetDataLength(ins.DmaBaseAddress, ins.ChannelIndex, len)
#define DMA_GET_LENGTH(ins) LL_DMA_GetDataLength(ins.DmaBaseAddress, ins.ChannelIndex)
#define DMA_SET_ADDRESS(ins, addr) LL_DMA_SetMemoryAddress(ins.DmaBaseAddress, ins.ChannelIndex, addr)

void uart_tx_poll(unsigned char hw_id) {
	if (DMA_IS_ENABLED(hdma_usart3_tx)) {
		if (SET != DMA_FLAGS_GET(&hdma_usart3_tx, DMA_FLAG_TC4)) {
			return;
		}
		circle_skip_read_size(tx_queue_, uart_tx_length);
		DMA_SET_ENABLE(hdma_usart3_tx, false);
	}

	uart_tx_length = circle_get_read_size(tx_queue_);
	if (uart_tx_length > 0) {
		DMA_SET_LENGTH(hdma_usart3_tx, uart_tx_length);
		DMA_SET_ADDRESS(hdma_usart3_tx, (u32) circle_get_read_head(tx_queue_));
		DMA_FLAGS_CLEAR(&hdma_usart3_tx, DMA_FLAG_TC4);
		if (rs485_rx_is_enabled()) {
			rs485_rx_enable(false);
		}
		DMA_SET_ENABLE(hdma_usart3_tx, true);
	}
	if (!DMA_IS_ENABLED(hdma_usart3_tx) && __HAL_UART_GET_FLAG(&huart3, UART_FLAG_TC)) {
		rs485_rx_enable(true);
		__HAL_UART_CLEAR_FLAG(&huart3, UART_CLEAR_TCF);
	}
}

void uart_rx_poll(unsigned char hw_id) {
	u16 index = uart_rx_index;

	uart_rx_index = sizeof(uart_rx_cache) - DMA_GET_LENGTH(hdma_usart3_rx);
	if (!rx_cb_) {
		return;
	}
	if (uart_rx_index < index) {
		rx_cb_(ctx_, uart_rx_cache + index, sizeof(uart_rx_cache) - index);
		rx_cb_(ctx_, uart_rx_cache, uart_rx_index);
	} else {
		rx_cb_(ctx_, uart_rx_cache + index, uart_rx_index - index);
	}
}

void uart_set_dma_queue(unsigned char hw_id, void *queue) {
	tx_queue_ = (c_buffer_t *)queue;
}

static void MX_USART3_UART_Init(void)
{

  /* USER CODE BEGIN USART3_Init 0 */

  /* USER CODE END USART3_Init 0 */

  /* USER CODE BEGIN USART3_Init 1 */

  /* USER CODE END USART3_Init 1 */
  huart3.Instance = USART3;
  huart3.Init.BaudRate = 115200;
  huart3.Init.WordLength = UART_WORDLENGTH_8B;
  huart3.Init.StopBits = UART_STOPBITS_1;
  huart3.Init.Parity = UART_PARITY_NONE;
  huart3.Init.Mode = UART_MODE_TX_RX;
  huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart3.Init.OverSampling = UART_OVERSAMPLING_16;
  huart3.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
  huart3.Init.ClockPrescaler = UART_PRESCALER_DIV1;
  huart3.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
  if (HAL_UART_Init(&huart3) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_SetTxFifoThreshold(&huart3, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_SetRxFifoThreshold(&huart3, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_EnableFifoMode(&huart3) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART3_Init 2 */

  /* USER CODE END USART3_Init 2 */

}

void HAL_UART_MspInit(UART_HandleTypeDef* huart)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
  if(huart->Instance==USART3)
  {
  /* USER CODE BEGIN USART3_MspInit 0 */

  /* USER CODE END USART3_MspInit 0 */

  /** Initializes the peripherals clocks
  */
    PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USART3;
    PeriphClkInit.Usart3ClockSelection = RCC_USART3CLKSOURCE_PCLK1;
    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
    {
      Error_Handler();
    }

    /* Peripheral clock enable */
    __HAL_RCC_USART3_CLK_ENABLE();

    __HAL_RCC_GPIOB_CLK_ENABLE();
    /**USART3 GPIO Configuration
    PB8-BOOT0     ------> USART3_RX
    PB9     ------> USART3_TX
    */
    GPIO_InitStruct.Pin = GPIO_PIN_9;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.Alternate = GPIO_AF7_USART3;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

	GPIO_InitStruct.Pin = GPIO_PIN_8;
	GPIO_InitStruct.Pull = GPIO_PULLUP;
	HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
	
    /* USART3 DMA Init */
    /* USART3_RX Init */
    hdma_usart3_rx.Instance = DMA1_Channel3;
    hdma_usart3_rx.Init.Request = DMA_REQUEST_USART3_RX;
    hdma_usart3_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
    hdma_usart3_rx.Init.PeriphInc = DMA_PINC_DISABLE;
    hdma_usart3_rx.Init.MemInc = DMA_MINC_ENABLE;
    hdma_usart3_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
    hdma_usart3_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
    hdma_usart3_rx.Init.Mode = DMA_CIRCULAR;
    hdma_usart3_rx.Init.Priority = DMA_PRIORITY_LOW;
    if (HAL_DMA_Init(&hdma_usart3_rx) != HAL_OK)
    {
      Error_Handler();
    }

    __HAL_LINKDMA(huart,hdmarx,hdma_usart3_rx);

    /* USART3_TX Init */
    hdma_usart3_tx.Instance = DMA1_Channel4;
    hdma_usart3_tx.Init.Request = DMA_REQUEST_USART3_TX;
    hdma_usart3_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
    hdma_usart3_tx.Init.PeriphInc = DMA_PINC_DISABLE;
    hdma_usart3_tx.Init.MemInc = DMA_MINC_ENABLE;
    hdma_usart3_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
    hdma_usart3_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
    hdma_usart3_tx.Init.Mode = DMA_NORMAL;
    hdma_usart3_tx.Init.Priority = DMA_PRIORITY_LOW;
    if (HAL_DMA_Init(&hdma_usart3_tx) != HAL_OK)
    {
      Error_Handler();
    }

    __HAL_LINKDMA(huart,hdmatx,hdma_usart3_tx);

  /* USER CODE BEGIN USART3_MspInit 1 */

  /* USER CODE END USART3_MspInit 1 */

  }

}


void HAL_UART_MspDeInit(UART_HandleTypeDef* huart)
{
  if(huart->Instance==USART3)
  {
  /* USER CODE BEGIN USART3_MspDeInit 0 */

  /* USER CODE END USART3_MspDeInit 0 */
    /* Peripheral clock disable */
    __HAL_RCC_USART3_CLK_DISABLE();

    /**USART3 GPIO Configuration
    PB8-BOOT0     ------> USART3_RX
    PB9     ------> USART3_TX
    */
    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_8|GPIO_PIN_9);

    /* USART3 DMA DeInit */
    HAL_DMA_DeInit(huart->hdmarx);
    HAL_DMA_DeInit(huart->hdmatx);
  /* USER CODE BEGIN USART3_MspDeInit 1 */

  /* USER CODE END USART3_MspDeInit 1 */
  }

}

